#include "DetectCover.h"
#include "logger.h"

DetectCover::DetectCover()
{
}


DetectCover::~DetectCover()
{
}

int DetectCover::init()
{
    YoloParam param;
    param.confidence_threshold = conf_thre;
    param.score_threshold = score_thre;
    param.nms_threshold = nms_thre;

    return 	detecter.init(modle_path, class_path, param, true);
}

int DetectCover::get_best_reslut(cv::Mat img, YoloDetection &best_reslut)
{
    std::vector<YoloDetection> detect_output;
    detecter.detect(img, detect_output);
    std::vector<YoloDetection> detect_output_filter;
    //判断box尺寸 若box  width height 过小则认为检测失败
    for(int i=0;i<detect_output.size();i++){
        YoloDetection detect = detect_output[i];
        cv::Rect rect = detect.box;
        if ((rect.width > 100 && rect.width < 300)  &&
            (rect.height > 100 && rect.height < 300)&&
            ((float)rect.width/rect.height < 1.3) && ((float)rect.width/rect.height>0.7))
        {
            detect_output_filter.push_back(detect);
        }
    }

    YoloDetection best_plate = { 0,0,{ 0,0,0,0 } };
    float max_confidence = 0;
    if (detect_output_filter.size() == 1){
        best_plate.box = detect_output_filter.at(0).box;
        best_plate.class_id = detect_output_filter.at(0).class_id;
        best_plate.confidence = detect_output_filter.at(0).confidence;
    }else if (detect_output_filter.size() > 1)
    {
        max_confidence = detect_output_filter.at(0).confidence;
        best_plate.box = detect_output_filter.at(0).box;
        best_plate.class_id = detect_output_filter.at(0).class_id;
        best_plate.confidence = detect_output_filter.at(0).confidence;

        for (int i = 1; i < detect_output_filter.size(); i++){
            if (max_confidence < detect_output_filter.at(i).confidence){
                max_confidence = detect_output_filter.at(i).confidence;
                best_plate.box = detect_output_filter.at(i).box;
                best_plate.class_id = detect_output_filter.at(i).class_id;
                best_plate.confidence = detect_output_filter.at(i).confidence;
            }
        }
    }else{
        LOG(ERROR) << " YOLO detection failed! " << std::endl;
        return -1;
    }

    LOG(INFO) << "max_confidence is: " << max_confidence << std::endl;
    LOG(INFO) << "best box is: " << best_plate.box << std::endl;
    LOG(INFO) << "best class is:  " << best_plate.class_id << std::endl;
    LOG(INFO) << "best confidence is: " << best_plate.confidence << std::endl;

    best_reslut = best_plate;
}

 bool DetectCover::yoloDetect(cv::Mat &img,cv::Rect& rect)
{
    YoloDetection best_reslut = {};
    int nRet = get_best_reslut(img, best_reslut);
    if (nRet)
    {
        std::cout << "cover_detecter.get_best_reslut() err!" << std::endl;
        return false;
    }
    rect=best_reslut.box;
    return true;
}

bool DetectCover::findCoverDistance(cv::Mat &img,cv::Rect& rect,float &outx, float &outy, float &outz)
{
    YoloDetection best_reslut = {};
    int nRet = get_best_reslut(img, best_reslut);
    if (nRet)
    {
        std::cout << "cover_detecter.get_best_reslut() err!" << std::endl;
        return false;
    }

    auto box = best_reslut.box;
    auto classId = best_reslut.class_id;

    cv::rectangle(img, box, { 255,0,0 }, 2);
    cv::rectangle(img, cv::Point(box.x, box.y - 20), cv::Point(box.x + box.width, box.y), { 50,255,150 }, cv::FILLED);

    cv::putText(img, detecter.mClassNames[classId].c_str(), cv::Point(box.x, box.y - 5), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
    cv::circle(img, { img.cols / 2,img.rows / 2 }, 3, { 255,0,0 }, -1, 8);

    float distance = -2.6264 * box.width + 1157.2;
    std::string str = "calc Z distance is: " + std::to_string(distance);

    cv::putText(img, str.c_str(), cv::Point(box.x, box.y - 25), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255));

    cv::Point2f p_center;
    p_center.x = box.x + box.width / 2;
    p_center.y = box.y + box.height / 2l;
    cv::circle(img, p_center, 3, { 0,0,255 }, -1, 8);

    float d_x, d_y;
    d_x = p_center.x - img.cols / 2;
    d_y = p_center.y - img.rows / 2;

    //X 方向比例   y = 0.0005x + 0.0586 XXXXX
    //Y 方向比例  y = 0.0006x + 0.0278  YYYYY

    //计算相对距离 距离= 像素数 * 比例
    float dist_x;
    float dist_y;

    dist_x = (0.0005 * distance + 0.0586) * d_x;
    dist_y = (0.0006 * distance + 0.0278) * d_y;

    std::string strX = "calc X distance is: " + std::to_string(dist_x);
    std::string strY = "calc Y distance is: " + std::to_string(dist_y);
    cv::putText(img, strX.c_str(), cv::Point(box.x, box.y - 75), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255));
    cv::putText(img, strY.c_str(), cv::Point(box.x, box.y - 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255));

    outx = dist_x;
    outy = dist_y;
    outz = distance;
    rect = box;
    return true;
}
